#include "quadraped_app.h"
#include "gait.h"
#include "leg.h"
#include "motor_device.h"
#include "cmsis_os.h"
#include "dt7.h"
#include "math.h"
#include "usb_comm_app.h"

void Leg_SetPos_Time(Leg_index_e leg_index, float x, float y, uint32_t time)
{
	Leg_SetPos(leg_index, x, y);
	osDelay(time);
	// for (int i = 0; i < time; i++)
	// {
	// 	Leg_SetPos(leg_index, x, y);
	// 	osDelay(1);
	// }

}

void move_forward()
{
	float avrg = (LF.x + RF.x + LB.x + RB.x)/4;
	Leg_SetPos(LEG_INDEX_LF, LF.x-avrg, LF.y);
	Leg_SetPos(LEG_INDEX_RF, RF.x-avrg, RF.y);
	Leg_SetPos(LEG_INDEX_LB, LB.x-avrg, LB.y);
	Leg_SetPos(LEG_INDEX_RB, RB.x-avrg, RB.y);
	osDelay(2000);
}

void auto_drive_sleg(Leg_index_e leg_index)
{
	
	Tar_point_t* ptar_point= tar_point+leg_index;
	
	// Leg_SetPos_Time(LEG_INDEX_LF, 0, 16, 1000);
	float x0 = leg[leg_index].x, y0 = leg[leg_index].y;
	float x1 = ptar_point->x, y1 = ptar_point->y;
	float x = x1, y = y1;
	if(x*x + y*y > 23*23 || x*x + y*y < 11.5*11.5 || y < 0) 
	{
		x1 = x0 + 7;
		y1 = y0;
	}
	Leg_SetPos_Time(leg_index, x0, y1-3, 2000);
	Leg_SetPos_Time(leg_index, x1, y1-3, 2000);
	Leg_SetPos_Time(leg_index, x1, y1, 2000);

	move_forward();
}

void auto_drive_blocking()
{
	while (RC_CtrlData.rc.RightSwitch == RC_SWITCH_TOP)
	{
		auto_drive_sleg(LEG_INDEX_LF);
		auto_drive_sleg(LEG_INDEX_RB);
		auto_drive_sleg(LEG_INDEX_RF);
		auto_drive_sleg(LEG_INDEX_LB);

		// osDelay(3000);
	}
	
}

float jscope_fdb;
float jscope_ref;
float x = 14;
float y = 14;


void Quadraped_App()
{
	/* 用于 osDelayUntil */
	uint32_t PreviousWakeTime; 

	
	for(;;)
	{
		/* save wakeup time */
		PreviousWakeTime = osKernelSysTick();
		float rotate_spd = 0, forward_spd = 0;
		
		forward_spd =  1.0f*RC_CtrlData.rc.RightY;
		rotate_spd  = -1.0f*RC_CtrlData.rc.RightX;
		jscope_ref = RF.m1.pos_pid.ref;
		jscope_fdb = RF.m2.pos_pid.ref;
		// continue;
		if(RC_CtrlData.rc.RightSwitch == RC_SWITCH_TOP)
		{
//			Leg_SetPos(LEG_INDEX_LF, x, y);
//			Leg_SetPos(LEG_INDEX_RF, x, y);
//			Leg_SetPos(LEG_INDEX_LB, x, y);
//			Leg_SetPos(LEG_INDEX_RB, x, y);
			auto_drive_blocking();
			// osMutexWait(usb_buf_mutexHandle, 30);
			// osMutexRelease(usb_buf_mutexHandle);

			// if(fabs(forward_spd) > 0.1 || fabs(rotate_spd) > 0.1)
			// {
			// 	Walk_Body(GAIT_WALK_HALFCIRCLE, forward_spd, rotate_spd, Quadraped_GetPhase(1600)); //Trot行进
			// }
			// else{
			// 	Stand_Body(); //站立
			// }
			//Walk_Body(GAIT_CIRCLE, forward_spd, rotate_spd, Quadraped_GetPhase()); //Walk行进
			//Leg_SetAngle(LEG_INDEX_RB, 30.f*RC_CtrlData.rc.RightY, 30.f*RC_CtrlData.rc.LeftY);
			//Jump_Test();
			
			//Leg_SetAngle(LEG_INDEX_LB, 30.f*RC_CtrlData.rc.RightY, 30.f*RC_CtrlData.rc.LeftY);
			//Trot_Body(GAIT_CIRCLE, forward_spd, rotate_spd, Quadraped_GetPhase());
		}
		
		if(RC_CtrlData.rc.RightSwitch == RC_SWITCH_MIDDLE)
		{
			if(fabs(forward_spd) > 0.1 || fabs(rotate_spd) > 0.1)
			{
				Trot_Body(GAIT_CIRCLE, forward_spd, rotate_spd, Quadraped_GetPhase(550)); //Trot行进
			}
			else{
				Stand_Body(); //站立
			}
		}
		if(RC_CtrlData.rc.RightSwitch == RC_SWITCH_BOTTOM && RC_CtrlData.rc.LeftSwitch == RC_SWITCH_MIDDLE)
		{
			/* 腿部复位 */
			for (int leg_index = 0; leg_index < 4; leg_index++)
			{
				Motor_SetPos(&leg[leg_index].m1, -leg[leg_index].m1.bsp.abs_angle_offset);
				Motor_SetPos(&leg[leg_index].m2, -leg[leg_index].m2.bsp.abs_angle_offset);
			}

		}
		osDelayUntil(&PreviousWakeTime, 10);// every 10ms
	}
	

}


void Jump_Test()
{
	float max_y = RC_CtrlData.rc.LeftY*7 + 14;
	uint32_t stretch_time = 5000, recovery_time = 2000;//300ms蹬腿，100ms收腿
	uint32_t t;
	uint32_t stretch_start_time = osKernelSysTick();

	int x = 0, y = 0;
	while ((t = osKernelSysTick() - stretch_start_time) <=  stretch_time)
	{
		//蹬腿
		y = max_y*(t/stretch_time);

		Leg_SetPos(LEG_INDEX_LF, x, y);
		Leg_SetPos(LEG_INDEX_RF, x, y);
		Leg_SetPos(LEG_INDEX_LB, x, y);
		Leg_SetPos(LEG_INDEX_RB, x, y);
		osDelay(5);// every 5ms
	}
	
	uint32_t recovery_start_time = osKernelSysTick();
	while ((t = osKernelSysTick() - recovery_start_time) <=  recovery_time)
	{
		//收腿
		y = max_y - max_y*(t/recovery_time);

		Leg_SetPos(LEG_INDEX_LF, x, y);
		Leg_SetPos(LEG_INDEX_RF, x, y);
		Leg_SetPos(LEG_INDEX_LB, x, y);
		Leg_SetPos(LEG_INDEX_RB, x, y);
		osDelay(5);// every 5ms
	}
	while (RC_CtrlData.rc.RightSwitch == RC_SWITCH_TOP) osDelay(100); //等待退出Jump
}



void Func_Test()
{
	

}
